#include "loc_correction/kalman_filter.h"


KalmanFilter::KalmanFilter(double process_noise, double measurement_noise, double estimated_error, double initial_value) {
        this->q = process_noise; // 过程噪声协方差
        this->r = measurement_noise; // 测量噪声协方差
        this->p = estimated_error; // 初始估计误差
        this->x = initial_value; // 初始估计值
    }

double KalmanFilter::update(double measurement) {
    // 预测阶段
    this->p += this->q;

    // 更新阶段
    double k = this->p / (this->p + this->r);
    this->x += k * (measurement - this->x);
    this->p *= (1 - k);

    return this->x;
}